#include "pid.h"

float PID(float Actual, float Target,PID_Handle* hpid)
{	
	float fb = 0;
	float t1,t2,t3;
	hpid->Error = Target - Actual;
	
	t1 = hpid->Kp*hpid->Error;
	
	hpid->Sum_E += (hpid->Error);
	t2 = hpid->Kd * hpid->Sum_E;
	
	t3 = hpid->Ki*(hpid->Error - hpid->PreError);
	
	fb = t1+t2+t3;
	
	
	
	if(fb >= 10 && hpid->Error > 0)
	{
		fb = 10;
		hpid->Sum_E -= hpid->Error;
		hpid->Error = 0;
	}
	if(fb <= -10 && hpid->Error < 0)
	{
		hpid->Sum_E -= hpid->Error;
		hpid->Error = 0;
		fb = -10;
	}
	
	hpid->PreError = hpid->Error;
	return fb;
}

